
// This file contains the helper functions to generate OPENDRIVE road dates and geometries
/*
OPENDRIVE convention:
side: -1 right, 0 center, 1 left
pos: -1 end, 1 head
link dir: -1 end, 1 start
lane side: 0 near, 1 far
*/

#pragma once
#include <glm/glm.hpp>
#include <vector>
#include <map>
#include <math.h>
#include <memory>
#include <string>

namespace ODRoadGeom
{
#ifndef PI
#define  PI   3.14159265358979323846
#endif

    struct ODCfgParameters
    {
        static  double step;
        static  double roadMarkStep;
        static  double uvlength;
    };

    using namespace std;

    class ODSystem;
    class ODRoad;
    class ODJunction;
    class ODElement;
    class ODLink;
    class ODController;
    class ODLaneSection;
    class ODLane;
    class ODConnection;
    class ODSignal;
    class ODSignalReference;
    class ODPlanCurve;
    class ODPolynomCurve;
    class ODLinearCurve;
    class ODRoadMarkCurve;

    // shared points
    typedef shared_ptr<ODSystem>            ODSystemPtr;
    typedef shared_ptr<ODRoad>              ODRoadPtr;
    typedef shared_ptr<ODJunction>          ODJunctionPtr;
    typedef shared_ptr<ODElement>           ODElementPtr;
    typedef shared_ptr<ODLink>              ODLinkPtr;
    typedef shared_ptr<ODController>        ODControllerPtr;
    typedef shared_ptr<ODLaneSection>       ODLaneSectionPtr;
    typedef shared_ptr<ODLane>              ODLanePtr;
    typedef shared_ptr<ODConnection>        ODConnectionPtr;
    typedef shared_ptr<ODSignal>            ODSignalPtr;
    typedef shared_ptr<ODSignalReference>   ODSignalRefPtr;
    typedef shared_ptr<ODPlanCurve>         ODPlanCurvePtr;
    typedef shared_ptr<ODPolynomCurve>      ODPolynomCurvePtr;
    typedef shared_ptr<ODLinearCurve>       ODLinearCurvePtr;
    typedef shared_ptr<ODRoadMarkCurve>     ODRoadMarkCurvePtr;

    // weak points
    typedef weak_ptr<ODRoad>                ODRoadRef;
    typedef weak_ptr<ODJunction>            ODJunctionRef;
    typedef weak_ptr<ODElement>             ODElementRef;
    typedef weak_ptr<ODLink>                ODLinkRef;

    /*
        OpenDrive plan view 
    */
    class ODPlanCurve
    {
    public:
        ODPlanCurve(double s, double in_xs, double in_ys, double in_hdg, double in_length)
            : start(s), length(in_length), xs(in_xs), ys(in_ys), hdg(in_hdg)
        {
            sinhdg = sin(in_hdg);
            coshdg = cos(in_hdg);
        }

        virtual ~ODPlanCurve() {}
    
    public:
        virtual glm::dvec3 GetPoint(double) = 0;
        virtual double GetOrientation(double) = 0;

    public:
        double          start;
        double          length;

    protected:
        double          xs;
        double          ys;
        double          hdg;
        double          sinhdg;
        double          coshdg;



    };

    class PlanLine : public ODPlanCurve
    {
    public:
        PlanLine(double s, double in_xs, double in_ys, double in_hdg, double in_length)
            : ODPlanCurve(s, in_xs, in_ys, in_hdg, in_length)
        {}

        virtual glm::dvec3 GetPoint(double s)
        {
            s -= start;
            return glm::dvec3(coshdg * s + xs, sinhdg * s + ys, hdg);
        }

        virtual double GetOrientation(double s) 
        {
            s -= start; 
            return hdg;
        }
    };

    class PlanArc : public ODPlanCurve
    {
    public:
        PlanArc(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_cr)
            : ODPlanCurve(s, in_xs, in_ys, in_hdg, in_length)
            , curvature(in_cr)
        {
            xs = in_xs - in_cr * sinhdg;
            ys = in_ys + in_cr * coshdg;
        }

        virtual glm::dvec3 GetPoint(double s)
        {
            s -= start;
            double t = s / curvature;
            double sint = sin(t);
            double cost = cos(t);
            return glm::dvec3(xs + curvature * (coshdg * sint + sinhdg * cost), ys + curvature * (sinhdg * sint - coshdg * cost), s / curvature + hdg);
        }

        virtual double GetOrientation(double s)
        {
            s -= start;
            return s / curvature + hdg;
        }
    private:
        double          curvature;

    };

    class PlanSpiral : public ODPlanCurve
    {
    public:
        PlanSpiral(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_cs, double in_ce)
            : ODPlanCurve(s, in_xs, in_ys, in_hdg, in_length)
            , curvStart(in_cs)
            , curvEnd(in_ce)
        {
            sqrtpi = sqrt(PI);
            double cdelta = curvEnd - curvStart;
            if (cdelta > 0)
            {
                ax = ay = sqrt(length / cdelta);
                ts = curvStart * ax / sqrtpi;
            }
            else if (cdelta < 0)
            {
                ax = sqrt(-length / cdelta);
                ay = -ax;
                ts = curvStart * ay / sqrtpi;
            }
            else
            {
                ax = ay = 0;
                ts = 0;
            }

            double chdg = ay / ax * PI * ts * ts * 0.5;
            double beta = hdg - chdg;
            hdgs = beta;
            sinbeta = sin(beta);
            cosbeta = cos(beta);

            double ts2 = pow(ts, 2);
            tsn[0] = ts;
            for (int i = 1; i < 9; ++i)
            {
                tsn[i] = tsn[i - 1] * ts2;
            }
            fca8 = 39 * pow(PI, 8) / ((double)6843432960);
            fca7 = 11 * pow(PI, 7) / ((double)106444800);
            fca6 = 11424 * pow(PI, 6) / ((double)6843432960);
            fca5 = 2520 * pow(PI, 5) / ((double)106444800);
            fca4 = 1980160 * pow(PI, 4) / ((double)6843432960);
            fca3 = 316800 * pow(PI, 3) / ((double)106444800);
            fca2 = 171085824 * pow(PI, 2) / ((double)6843432960);
            fca1 = 17740800 * PI / ((double)106444800);
            fca0 = 6843432960 / ((double)6843432960);
        }

        virtual glm::dvec3 GetPoint(double s)
        {
            s -= start;
            double t = s / (ax * sqrtpi) + ts;
            double x, y;
            if (s == 0 || abs(s - length) < 0.00001f)
                approximateClothoid(t, x, y);
            else
                integrateClothoid(t, x, y);
            return glm::dvec3(xs + x * cosbeta - y * sinbeta, ys + x * sinbeta + y * cosbeta, hdgs + ay / ax * 0.5 * PI * t * t);
        }

        virtual double GetOrientation(double s)
        {
            s -= start;
            double t = s / (ax * sqrtpi) + ts;
            return hdg + ay / ax * 0.5 * PI * t * t;
        }

    private:
        void approximateClothoid(double t, double &x, double &y)
        {
            double t2 = pow(t, 2);
            double tn[9];
            tn[0] = t;
            for (int i = 1; i < 9; ++i)
            {
                tn[i] = tn[i - 1] * t2;
            }

            x = ax * sqrtpi * (fca8 * (tn[8] - tsn[8]) - fca6 * (tn[6] - tsn[6]) + fca4 * (tn[4] - tsn[4]) - fca2 * (tn[2] - tsn[2]) + fca0 * (tn[0] - tsn[0]));
            y = ay * sqrtpi * (fca7 * (tn[7] - tsn[7]) + fca5 * (tn[5] - tsn[5]) - fca3 * (tn[3] - tsn[3]) + fca1 * (tn[1] - tsn[1]));
        }

        void integrateClothoid(double t, double &x, double &y)
        {
            x = 0;
            y = 0;

            double h = 0.0001;

            //std::cout << "t-ts: " << t-ts << std::endl;
            double n = ceil((t - ts) / h);

            h = (t - ts) / n;

            if (h == h)
            {
                for (int i = 1; i < (n - 1); ++i)
                {
                    x += cos(PI * 0.5 * pow(ts + i * h, 2));
                    y += sin(PI * 0.5 * pow(ts + i * h, 2));
                }
                x += (cos(PI * ts) + cos(PI * t)) * 0.5;
                y += (sin(PI * ts) + sin(PI * t)) * 0.5;
                x *= ax * sqrtpi * h;
                y *= ay * sqrtpi * h;
            }
        }

    private:
        double          curvStart;
        double          curvEnd;

        double ax, ay;
        //double xs, ys;
        double sinbeta, cosbeta;
        double ts;
        double hdgs;

        double tsn[9];
        double fca8, fca7, fca6, fca5, fca4, fca3, fca2, fca1, fca0;

        double sqrtpi;

    };

    class PlanPoly3 : public ODPlanCurve
    {
    public:
        PlanPoly3(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_a, double in_b, double in_c, double in_d)
            : ODPlanCurve(s, in_xs, in_ys, in_hdg, in_length)
            , a(in_a), b(in_b), c(in_c), d(in_d)
        {}

        virtual glm::dvec3 GetPoint(double s)
        {
            s -= start;
            double t = getT(s);

            double u = t;
            double v = (a + b * t + c * t * t + d * t * t * t);
            return glm::dvec3(u * coshdg - v * sinhdg + xs, u * sinhdg + v * coshdg + ys, atan(3 * d * t * t + 2 * c * t + b) + hdg);
        }

        virtual double GetOrientation(double s)
        {
            s -= start;
            double t = getT(s);
            return atan(3 * d * t * t + 2 * c * t + b) + hdg;
        }

    private:
        double getT(double s)
        {
            int n = 100;
            double h = s / n;
            double t = 0;
            double dt = 0;
            for (int i = 0; i < n; ++i)
            {
                dt = 1 / sqrt(pow((3 * d * t * t + 2 * c * t + b), 2) + 1) * h;
                t += dt;
            }
            return t;
        }

    private:
        double          a, b, c, d;
    };

    class PlanParamPoly3 : public ODPlanCurve
    {
    public:
        PlanParamPoly3(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_aU, double in_bU, double in_cU, double in_dU, double in_aV, double in_bV, double in_cV, double in_dV, double in_pRange)
            : ODPlanCurve(s, in_xs, in_ys, in_hdg, in_length)
            , aU(in_aU), bU(in_bU), cU(in_cU), dU(in_dU)
            , aV(in_aV), bV(in_bV), cV(in_cV), dV(in_dV)
            , pRange(in_pRange)
        {}

        virtual glm::dvec3 GetPoint(double s)
        {
            s -= start;
            double pos = getT(s);//(s / length) * pRange;
            double x = aU + bU * pos + cU * pos * pos + dU * pos * pos * pos;
            double y = aV + bV * pos + cV * pos * pos + dV * pos * pos * pos;
            double xrot = x * coshdg - y * sinhdg;
            double yrot = x * sinhdg + y * coshdg;
            double dx = bU + cU * pos + dU * pos * pos;
            double dy = bV + cV * pos + dV * pos * pos;
            double tangent = atan2(dy, dx);
            return glm::dvec3(xs + xrot, ys + yrot, hdg + tangent);
        }

        virtual double GetOrientation(double s)
        {
            s -= start;
            double t = getT(s);
            return hdgRad(t);
        }
    private:
        double getT(double s)
        {
            if (pRange == 1.0f)
            {
                double t = s;
                for (int i = 0; i < 20; ++i)
                {
                    // New code with integration //

                    //
                    double f = getParametricCurveLength(0.0, t) - s;

                    double df = sqrt((bU + 2.0 * cU * t + 3.0 * dU * t * t) * (bU + 2.0 * cU * t + 3.0 * dU * t * t) + (bV + 2.0 * cV * t + 3.0 * dV * t * t) * (bV + 2.0 * cV * t + 3.0 * dV * t * t));

                    t -= f / df;

                }
                return t;
            }
            else
                return s;
        }

        double getParametricCurveLength(double from, double to)
        {
            double factor = (to - from) / 2.0;

            double l = 0.568888888888889 * parametricF(0.0, factor) + 0.4786286704993665 * parametricF(-0.5384693101056831, factor)

                + 0.4786286704993665 * parametricF(0.5384693101056831, factor) + 0.2369268850561891 * parametricF(-0.906179845938664, factor)

                + 0.2369268850561891 * parametricF(0.9061798459386640, factor);

            l = l * factor;

            return l;
        }

        double parametricF(double t, double factor)
        {

            t = t * factor + factor;

            double q = sqrt((bU + 2.0 * cU * t + 3.0 * dU * t * t) * (bU + 2.0 * cU * t + 3.0 * dU * t * t) + (bV + 2.0 * cV * t + 3.0 * dV * t * t) * (bV + 2.0 * cV * t + 3.0 * dV * t * t));

            return q;

        }

        double hdgRad(double t)
        {
            return atan2((3 * dV * t * t + 2 * cV * t + bV), (3 * dU * t * t + 2 * cU * t + bU));
        }

    private:
        double          aU, bU, cU, dU;
        double          aV, bV, cV, dV;
        double          pRange;
    };

    /*
        OPENDRIVE Dates
    */
    class ODPolynomCurve
    {

    public:
        ODPolynomCurve(double in_s, double in_a, double in_b, double in_c, double in_d)
            : start(in_s), a(in_a), b(in_b), c(in_c), d(in_d)
        {}

        double GetValue(double s)
        {
            s -= start;
            return a + b * s + c * s * s + d * s * s * s;
        }

        double GetSlope(double s)   // get derivative
        {
            s -= start;
            return atan(b + 2 * c * s + 3 * d * s * s);
        }

    public:
        double          start;
    private:
        double          a, b, c, d;

    };

    class ODLinearCurve
    {

    public:
        ODLinearCurve(double s, double ier, double oer)
            : start(s), inner(ier), outer(oer)
        {}

        double GetValue(int i)
        {
            return (i == 0) ? inner : outer;
        }

    public:
        double              start;
    private:
        double              inner, outer;

    };


    /*
        OPENDRIVE elements
    */
    class ODElement
    {
    public:
        ODElement(const string&, const string&);
        string GetId() const;
        string GetName() const;
        virtual ~ODElement() {}

    public:
        string id;
        string name;
    };

    class ODController : public ODElement
    {

    public:
        ODController(const string&, const string&, int32_t);

    public:
        int32_t         sequence;
        struct Control
        {
            string     signalId;
            string     type;

            Control(const string& id, const string& t)
            {
                signalId = id;
                type = t;
            }
        };

        vector<Control>    controls;

    };

    // ODRoad
    class ODLink
    {
    public:
        enum LinkType
        {
            ELT_UNKNOWN,
            ELT_ROAD,
            ELT_JUNCTION,
        };

        ODLink(ODLink::LinkType t, const string& lk, int32_t in_dir)
            : linkType(t), link(lk), dir(in_dir)
        {}

        //void CollectLink(std::weak_ptr<ODRoad> road, int32_t side, int32_t pos, string& out_id, int32_t& out_side, int32_t& out_pos, int32_t& out_weight);
    public:
        LinkType        linkType;
        string          link;
        int32_t         dir;
    };

    class ODRoad : public ODElement
    {
    public:

        ODRoad(const string&, const string&);
        ~ODRoad() {};

        // add plan view
        void AddPlanLine(double s, double in_xs, double in_ys, double in_hdg, double in_length);
        void AddPlanArc(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_cr);
        void AddPlanSpiral(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_cs, double in_ce);
        void AddPlanPoly3(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_a, double in_b, double in_c, double in_d);
        void AddParamPoly3(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_aU, double in_bU, double in_cU, double in_dU, double in_aV, double in_bV, double in_cV, double in_dV, double in_pRange);

        // add elevation
        void AddElevation(double in_s, double in_a, double in_b, double in_c, double in_d);

        // add lane section
        void AddLaneSection(ODLaneSectionPtr ls);

        // add signal
        void AddSignalRef(double s, double t, const string& id, const string& orientation);

        // evaluation
        void EvaluatePoint(double s, glm::dvec3& pos);
        void EvaluatePoint2D(double s, glm::dvec3& pos);
        double EvaluateElevation(double s);
        double EvaluateSlope(double s);

        void EvaluatePoint(double s, double t, glm::dvec3& pos);        
        double EvaluateWidth(double s, int32_t side);
        double EvaluateWidth(double s, int32_t side, const string& laneType);

        double GetOrientation(double s);

        // 
        ODPlanCurvePtr                  GetGeometry(double);
        ODPolynomCurvePtr               GetElevation(double);
        ODLaneSectionPtr                GetLaneSection(double);

        // geometries dates
        void GenerateVertices();
        void GenerateRoadMarks();

        // collect geometries dates
        void CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats) const;

        void CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats) const;

        const vector<glm::dvec3>& CollectCenter() const;

        void CollectBoundary(int32_t side, vector<glm::dvec3>& array, std::uint32_t& count, vector<int32_t>* indices = nullptr, vector<glm::dvec3>* normals = nullptr, vector<glm::dvec2>* uvs = nullptr);

        //// geometry information
        glm::dvec3 GetEdgePoint(int32_t side, int32_t pos);

        glm::dvec3 GetHeadPoint(int32_t pos);

        void FindClosetPointWith(const glm::dvec3& input, glm::dvec3& outpoint);

        //void CollectLink(int32_t side, int32_t pos, string& out_id, int32_t& out_side, int32_t& out_pos, int32_t& out_weight);

        //void GetEdgePointbyRef(int32_t side, int32_t pos, glm::dvec3& out_point, const ODElement& refObj);

        //int32_t GetHeadPointbyRef(glm::dvec3& out_point, const ODElement& refObj, ODLink::LinkType type = ODLink::ELT_JUNCTION);

        int32_t GetHeadCenterPointbyRef(glm::dvec3& out_point, const ODElement& refObj, ODLink::LinkType type = ODLink::ELT_JUNCTION);

        //void GetLinkbyRef(int32_t& out_side, int32_t& out_pos, int32_t ccd, const ODJunction& junction);

        void GetLinkEdgePointByRef(int32_t side, glm::dvec3& out_point, const ODJunction& junction);

        //bool IsOuterLane(int32_t laneid, int32_t pos);
        //int32_t GetOuterLaneId(int32_t side, int32_t pos);

        int32_t FindLaneLinkedIdWith(const string& linked, int32_t inId);

        // logic
        // return linked pos: -1 successor, 1 predecessor, 0 not linked
        int32_t    IsConnected(const string& linked, ODLink::LinkType type);

        //bool    HasLinked();

    public:
        double                          length;
        string                          junctionId;
        ODLinkPtr                       predecessor;
        ODLinkPtr                       successor;

        // road elements
        vector<ODPlanCurvePtr>          geometries;
        vector<ODPolynomCurvePtr>       elevations;
        vector<ODLaneSectionPtr>        laneSections;
        vector<ODSignalRefPtr>          signalRefs;

        // road geometry dates
        vector<glm::dvec3>                   centerRoad;
    };

    class ODLaneSection
    {
    public:
        ODLaneSection(double);
        ~ODLaneSection() {};

        void AddLane(ODLanePtr);
        double GetLaneWidth(double, int);
        double GetSideWidth(double, int32_t);
        double GetSideWidth(double, int32_t, const string&);
        double GetDistanceToLane(double, int);
        double GetHightInnerOffset(double, int);
        double GetHightOuterOffset(double, int);
        void CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats);
        void CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats);
        void GetEdgePoint(int32_t side, int32_t pos, glm::dvec3& out_point);
        void CollectBoundary(int32_t side, vector<glm::dvec3>& array, std::uint32_t& count, vector<int32_t>* indices, vector<glm::dvec3>* normals, vector<glm::dvec2>* uvs) const;
        void FindClosetPointWith(const glm::dvec3& input, glm::dvec3& outpoint);
        int32_t GetOuterLaneId(int32_t side);
        int32_t FindLaneLinkedIdWith(int32_t laneid);

    public:
        double                     start;
        map<int, ODLanePtr>        LeftlaneMap;
        map<int, ODLanePtr>        CenterlaneMap;
        map<int, ODLanePtr>        RightlaneMap;
    };

    class ODLane
    {
    public:
        ODLane(int, const string&, const string&);
        ~ODLane() {};

        void AddWidth(double, double, double, double, double);
        double GetWidth(double);

        void AddHeight(double, double, double);
        double GetHeight(double, int i);

        void AddRoadMark(double in_start, string stype, string sweight,
            string scolor, string material, double in_width,
            string slaneChange, double in_height);

        void CollectRoadMark(double s, int i, const glm::dvec3& ref, const glm::dvec3& refN, double Tx, double Ty, double Tz);

        ODPolynomCurvePtr   GetWidthCurve(double s);
        ODLinearCurvePtr    GetHeightCurve(double s);
        ODRoadMarkCurvePtr  GetRoadmarkCurve(double s);

        void GenerateLanePoint(double s, int l, ODLaneSectionPtr owner, ODRoad& road, bool lastRound);
        void GenerateRoadMark(double s, int l, ODLaneSectionPtr owner, ODRoad& road);

        void CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, std::uint32_t laneside, vector<string>& mats);
        void CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, std::uint32_t laneside, vector<string>& mats);
        void FindClosetPointWith(const glm::dvec3& input, glm::dvec3& outpoint);
    public:
        int                             id;
        string                          laneType;
        bool                            level;
        int32_t							predecessor;
        int32_t							successor;

        vector<ODPolynomCurvePtr>       widths;
        vector<ODLinearCurvePtr>        heights;
        vector<ODRoadMarkCurvePtr>      roadmarks;

        vector<glm::dvec3>                    nears;
        vector<glm::dvec3>                    nearNormals;
        vector<glm::dvec2>                    nearUVs;

        vector<glm::dvec3>                    fars;
        vector<glm::dvec3>                    farNormals;
        vector<glm::dvec2>                    farUVs;

        vector<glm::dvec3>                    highnears;
        vector<glm::dvec3>                    highnearnormals;
        vector<glm::dvec2>                    highnearuvs;
        vector<glm::dvec3>                    lownears;
        vector<glm::dvec3>                    lownearnormals;
        vector<glm::dvec2>                    lownearuvs;

        vector<glm::dvec3>                    highfars;
        vector<glm::dvec3>                    highfarnormals;
        vector<glm::dvec2>                    highfaruvs;
        vector<glm::dvec3>                    lowfars;
        vector<glm::dvec3>                    lowfarnormals;
        vector<glm::dvec2>                    lowfaruvs;


    public:
        static const int                NOLANE;
    };

    class ODRoadMarkCurve
    {
    public:
        enum ERoadMarkType
        {
            ERM_None,
            ERM_Solid,
            ERM_Broken,
            ERM_SolidSolid,
            ERM_SolidBroken,
            ERM_BrokenSolid,
            ERM_BrokenBroken,
            ERM_BottsDots,
            ERM_Grass,
            ERM_Curb,
        };

        enum ERoadMarkWeight
        {
            ERW_Standard,
            ERW_Bold,
        };

        enum ERoadMarkColor
        {
            ERC_Standard,
            ERC_Blue,
            ERC_Green,
            ERC_Red,
            ERC_White,
            ERC_Yellow,
        };

        enum ERoadLaneChange
        {
            ERL_Increase,
            ERL_Decrease,
            ERL_Both,
            ERL_None,
        };

    public:
        ODRoadMarkCurve(double in_start, string stype, string sweight,
            string scolor, string in_material, double in_width,
            string slaneChange, double in_height)
            :start(in_start), width(in_width), height(in_height)
        {
            if (stype == string("none"))
            {
                type = ERM_None;
            }
            else if (stype == string("solid"))
            {
                type = ERM_Solid;
            }
            else if (stype == string("broken"))
            {
                type = ERM_Broken;
            }
            else if (stype == string("solid solid"))
            {
                type = ERM_SolidSolid;
            }
            else if (stype == string("solid broken"))
            {
                type = ERM_SolidBroken;
            }
            else if (stype == string("broken solid"))
            {
                type = ERM_BrokenSolid;
            }
            else if (stype == string("broken broken"))
            {
                type = ERM_BrokenBroken;
            }
            else if (stype == string("botts dots"))
            {
                type = ERM_BottsDots;
            }
            else if (stype == string("grass"))
            {
                type = ERM_Grass;
            }
            else if (stype == string("curb"))
            {
                type = ERM_Curb;
            }
            else
            {
                type = ERM_None;
            }

            if (sweight == string("standard"))
            {
                weight = ERW_Standard;
            }
            else if (sweight == string("bold"))
            {
                weight = ERW_Bold;
            }

            if (scolor == string("standard"))
            {
                color = ERC_Standard;
            }
            else if (scolor == string("blue"))
            {
                color = ERC_Blue;
            }
            else if (scolor == string("green"))
            {
                color = ERC_Green;
            }
            else if (scolor == string("red"))
            {
                color = ERC_Red;
            }
            else if (scolor == string("white"))
            {
                color = ERC_White;
            }
            else if (scolor == string("yellow"))
            {
                color = ERC_Yellow;
            }
            else
            {
                color = ERC_White;
            }

            if (slaneChange == string("increase"))
            {
                laneChange = ERL_Increase;
            }
            else if (slaneChange == string("decrease"))
            {
                laneChange = ERL_Decrease;
            }
            else if (slaneChange == string("both"))
            {
                laneChange = ERL_Both;
            }
            else if (slaneChange == string("none"))
            {
                laneChange = ERL_None;
            }
            else
            {
                laneChange = ERL_Both;
            }

            material = in_material;
        }

        int GetOffsetIndex(double s);

        void CollectRoadMark(double s, const glm::dvec3& ref, const glm::dvec3& refN, double Tx, double Ty, double Tz);

        void CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, std::uint32_t side, vector<string>& mats) const;

        double                  start;
        ERoadMarkType           type;
        ERoadMarkWeight         weight;
        ERoadMarkColor          color;
        string                  material;
        double                  width;
        ERoadLaneChange         laneChange;
        double                  height;

        vector<vector<glm::dvec3>>            rm_nears;
        vector<vector<glm::dvec3>>            rm_fars;

        vector<vector<glm::dvec3>>            rm_Normals;

        vector<vector<glm::dvec2>>            rm_nearUVs;
        vector<vector<glm::dvec2>>            rm_farUVs;

    public:
        static const double             RoadMarkBroken;
        static const double             MinRoadMarkWidth;
    };

    /*
        OPENDRIVE Signal
    */

    class ODSignal
    {
    public:
        ODSignal(
            double                  s,
            double                  t,
            const string&          id,
            const string&          name,
            const string&          bDynamic,
            const string&          orientation,
            double                  zOffset,
            const string&          country,
            const string&          type,
            const string&          subtype,
            double                  value,
            const string&          unit,
            double                  height,
            double                  width,
            const string&          text,
            double                  hOffset,
            double                  pitch,
            double                  roll
        );
    public:
        enum ESOrtType
        {
            EST_Positive,
            EST_Negative,
            EST_Both,
        };

        double                  s;
        double                  t;
        string                 id;
        string                 name;
        bool                    bDynamic;
        ESOrtType               orientation;
        double                  zOffset;
        string                 country;
        string                 type;
        string                 subtype;
        double                  value;
        string                 unit;
        double                  height;
        double                  width;
        string                 text;
        double                  hOffset;
        double                  pitch;
        double                  roll;

    };

    class ODSignalReference
    {
    public:
        ODSignalReference(double s, double t, const string& id, const string& orientation);
    public:
        double                  s;
        double                  t;
        string                 id;
        ODSignal::ESOrtType       orientation;
    };

    /*
        OPENDRIVE JUNCTION
    */
    class ODConnection
    {
    public:
        ODConnection(const string&, const string&, const string&, const string&);

        void AddLaneLink(int, int);
        void FindConnectionRoad(const string& incoming, int32_t& curLane, int32_t side, int32_t pos, string& out_id, int32_t& out_side, int32_t& out_pos);
        int32_t FindRoadOuterLaneId(int32_t side);
        int32_t FindLinkOuterLandId(int32_t side);
    public:
        string                          id;
        string                          inRoad;
        string                          outRoad;
        int32_t                         dir;
        std::map<int32_t, int32_t>      lanelinkmap;
    };


    /* Junction */
    class ODJunction : public ODElement
    {
    public:
        ODJunction(const string&, const string&);

        void AddConnection(ODConnectionPtr);
        void AddController(const string&, const string&);

        // geometries dates
        void GenerateVertices(ODSystem& odroot);

        void CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats);

        void CollectJunctionRoad(map<string, ODRoadRef >&    junctionRoads, ODSystem& odroot );
        void CollectInComingRoad(map<string, ODRoadRef >&    incomingRoads, ODSystem& odroot );
        void CollectOutRoad(map<string, ODRoadRef >&    outRoads, const map<string, ODRoadRef >&    incomingRoads, ODSystem& odroot);

        string FindConnectRoad(const string& in, const string& out, ODSystem& odroot);

        void FindConnectionRoad(const string& incoming, int32_t side, int32_t pos, string& out_id, int32_t& out_side, int32_t& out_pos);
        void FindNeighborRoad(std::weak_ptr<ODRoad> road, const string& incoming, int32_t side, int32_t pos, string& out_id, int32_t& out_side, int32_t& out_pos);
        

        void CheckCollectLink(const string& roadid, string& out_id);

        int32_t FindRoadOuterLaneId(const string& incoming, const string& connRoad, int32_t side);
        int32_t FindLinkOuterLandId(const string& incoming, const string& connRoad, int32_t side);

        bool IsLinkedByOtherConnection(const string& incoming, const string& conn, int32_t laneid);
        ODConnectionPtr GetConnection(const string& incoming, const string& conn);
        int32_t FindConnLaneId(const string& incoming, const string& conn, int32_t laneid);

        // new logic: road and juncition
        // face to the junction, get road's right side
        // return: -1 rightside, 1 leftside, 0 not linked with junciton
        int32_t GetInComingOuterSideCCW(const string& incoming, ODSystem& odroot) const;

    private:
        void    SortRoadsCCW(map<string, ODRoadRef> incomingRoads, vector<std::pair<string, float> >& thetas, glm::dvec3& center);
    public:
        map<string, ODConnectionPtr >   ConnectionMap;
        map<string, string >            ControllerMap;
        vector<glm::dvec3>                   verticesCCW;
    };

    /*
        OPENDRIVE System
    */
    class ODSystem
    {
    public:
        static ODSystem*    Instance();
        static void         Destroy();
        ~ODSystem() {};

    protected:
        ODSystem();

    public:
        ODRoadPtr       GetRoad(const string& id) const;
        ODJunctionPtr   GetJunction(const string& id) const;
        ODSignalPtr     GetSignal(const string& id) const;

        void            GenerateVertices();

        // add signal
        void AddSignal(
            double                  s,
            double                  t,
            const string&          id,
            const string&          name,
            const string&          bDynamic,
            const string&          orientation,
            double                  zOffset,
            const string&          country,
            const string&          type,
            const string&          subtype,
            double                  value,
            const string&          unit,
            double                  height,
            double                  width,
            const string&          text,
            double                  hOffset,
            double                  pitch,
            double                  roll
        );

    private:
        static ODSystem*                    _instance;

    public:
        map<string, ODRoadPtr>              roadMap;
        map<string, ODJunctionPtr>          junctionMap;
        map<string, ODSignalPtr>            signalMap;
        map<string, ODControllerPtr>        ctrlMap;
        wstring                             Name;

    };
}
